clc
clear
load('F3.mat')
% 设置目标点
lon0 = 50.5*pi/180;
lat0 = 50.5*pi/180;
% 再入段末端状态量lon, lat, alt, v, fpa, azi
entryStatef = [lon(end),lat(end), alt(end), V(end), fpa(end), azi(end)];
% entryStatef = [lon(end)*pi/180,lat(end)*pi/180,alt(end)*1000,V(end),fpa(end)*pi/180,azi(end)*pi/180];
% entryStatef = [50.021251030101311*pi/180,49.9698025384262*pi/180, 28.577845395811767e3,...
%     2.006754890721096e+03, -0.823311189767374*pi/180, 61.904083878542522*pi/180];
lla = [entryStatef(1),entryStatef(2),entryStatef(3)];
XYZ = lla2ECEF(lla);
ENU = XYZ2ENU(XYZ, lat0, lon0);
% 末段初始状态
state0 = [ENU(1) ENU(2) ENU(3) entryStatef(4) entryStatef(5) entryStatef(6)];